/*
 * MultitouchOscReceiver.h
 *
 *      Author: sashikanth
 */

#ifndef MULTITOUCHOSCRECEIVER_H_
#define MULTITOUCHOSCRECEIVER_H_

#include <vector>
#include <algorithm>

#include "osc/OscReceivedElements.h"
#include "osc/OscOutboundPacketStream.h"
#include "osc/OscPacketListener.h"
#include "ip/UdpSocket.h"
#include "gesture/OscGestureRecognition.h"
#include "gesture/Gestures.h"


#define ADDRESS "127.0.0.1"
#define OUTPUT_BUFFER_SIZE 4096
#define IN_PORT 3330
#define OUTPUT_PORT 3333

using namespace std;
ContactSetFrame frame;
class OscHandler: public osc::OscPacketListener
{
public:

	ContactSetFrame gestrFrame;
	ContactSetFrame liveFrame;
	OscGestureRecognition *listener;
	RecognitionHelper *recognizer;
	osc::OutboundPacketStream* outStream;
	UdpListeningReceiveSocket* inSock;
	bool gestrSampleStart;
	char buffer[OUTPUT_BUFFER_SIZE];
	//The ids that we have in store after processing the frame.
	vector<int> liveIds;
	int xRes, yRes;

	//For Boost::python to compile
	OscHandler()
	{
		outStream = new osc::OutboundPacketStream(buffer, OUTPUT_BUFFER_SIZE);
		gestrSampleStart = false;

	}

	OscHandler(int xRes, int yRes)
	:xRes(xRes), yRes(yRes)
	{
		cout << "Initing osc streams" << endl;
		outStream = new osc::OutboundPacketStream(buffer, OUTPUT_BUFFER_SIZE);
//
//		cout << "Now Listening for input on port " << IN_PORT << "..." << endl;
//
//		cout << "Output packets to port: " << OUTPUT_PORT << endl;
//
		cout << "Scaling input to resolution: " << xRes << "x" << yRes << endl;
//
//		inSock = &s;

		gestrSampleStart = false;
	}

	~OscHandler()
	{
	}

	void run()
	{
		inSock->RunUntilSigInt();
	}

	void handleGestrSample(osc::ReceivedMessageArgumentIterator & arg,
			const osc::ReceivedMessage & m)
	{
		const char *smpl = (arg++)->AsString();
		if (strcmp(smpl, "start") == 0)
		{
			const char *gestureName = (arg++)->AsString();
			gestrSampleStart = true;
			listener->startSample(gestureName);
		}
		else if (strcmp(smpl, "end") == 0)
		{
			gestrSampleStart = false;
			listener->endSample();
		}
		else if (strcmp(smpl, "set") == 0)
		{
			Contact contact = createContactFromOscArgs(arg, m);
			gestrFrame.push_back(contact);
		}
		else if (strcmp(smpl, "fseq") == 0)
		{
			listener->updateFrame(gestrFrame);
			gestrFrame.clear();
		}
		else
			cout << "Unsupported message: " << smpl << endl;

	}

	void handleGestrAction(osc::ReceivedMessageArgumentIterator & arg,
			const osc::ReceivedMessage & m)
	{
		const char *actionString 	= (arg++)->AsString();
		vector<string> actionParams;
		while (arg != m.ArgumentsEnd())
			actionParams.push_back((arg++)->AsString());

		vector<string> actionResult = listener->gestureAction(actionString, actionParams);
		sendGestrActionResults(actionString, actionResult);
	}

	void handleTUIO(osc::ReceivedMessageArgumentIterator arg,
			const osc::ReceivedMessage & m)
	{
		const char *param = (arg++)->AsString();
		vector<int> currIds;

		if (strcmp(param, "set") == 0)
		{
			Contact contact = createContactFromOscArgs(arg, m);
			//cloneContactToOutStream(contact);
			liveFrame.push_back(contact);
		}
		else if (strcmp(param, "alive") == 0)
		{
			//These are the ids from the current frame.
			currIds = getCurrIds(arg, m);

			//Check for change in fingers on screen.
			bool fingersRaised = isFingerRaised(currIds);
			bool fingersLowered = isFingerLowered(currIds);

			if(fingersLowered)
			{
				recognizer->resetOrderingNow();
				//cout << "FingersLowered. Num fingers now: " << currIds.size()  << endl;
				listener->startSample("");
				if(recognizer->isParameterizationPaused())
					recognizer->unpauseParameterization();
				fingerLowered();
			}
			if(fingersRaised)
			{
				recognizer->resetOrderingNow();
				//cout << "FingersRaised. Num fingers now: " << currIds.size() << endl;
				if(recognizer->isCurrentlyParameterized())
				{
					recognizer->clearPrevValsInParameterization();
					if(currIds.size() == 0) //All fingers raised. End gesture.
					{
						recognizer->unParameterize();
						listener->startSample("");
					}
					//Here the assumption is that the number of fingers to parameterize are the only ones being raised
					//FIXME: This logic will break when additional fingers have been lowered, and then raised --
					// -- leaving the fingers performing the parameterization on screen, but paused.
					else //Gesture is alive, but parameterization is paused
						recognizer->pauseParameterization();
				}
				fingerRaised();
			}
			liveIds = currIds;

			//We don't create a local cache of the sampleSize, becuase it changes with calls
			if(currIds.size() == 0 && listener->sampleSize() < 10)
			{
				listener->clearSample();
				return; //Had nothing, have nothing worth checking, nothing to do.
			}

			if(!recognizer->isCurrentlyParameterized()) //If the gesture is currently being parameterized, classification temporarily ceases.
			{
				segmentAndClassify(fingersRaised);
			}
		}
		else if (strcmp(param, "fseq") == 0)
		{
			//cout << "At Fseq: frameSize: " << liveFrame.size() << " liveIds: " << liveIds.size() << endl;
			if (!gestrSampleStart && liveFrame.size() > 0 && liveFrame.size() == liveIds.size())
			{
				listener->updateFrame(liveFrame);
				if(recognizer->isCurrentlyParameterized())
				{
					liveFrame = recognizer->reOrderFrame(liveFrame);
					sendGestrParams(recognizer->parameterize(liveFrame));
				}
			}
			//cloneFrameToOutStream(liveFrame);
			int fseq = (arg++)->AsInt32();
			sendOutStream(liveFrame, liveIds, fseq);
			liveFrame.clear();
		}
	}

	void segmentAndClassify(bool fingersRaised)
	{
		bool currSegmentIsNowStatic = false;

		if(listener->sampleSize() > 10) 			//Don't check if only 10 frames are collected
		{
			if (listener->sampleIsNowStatic()) 		//Check to see if sample has stopped moving
			{
				if(listener->sampleIsOnlyStatic()) 	//Entire sample hasn't moved.
					listener->startSample(""); 		//Restart collection of segment
				else 	//Delimiter found. Sample is valid if non-static portion of gesture is > 10 frames
				{
					currSegmentIsNowStatic = listener->sampleSize() > 10;
				}
			}
			//Two delimiters. currSegmentIsNowStatic or fingersRaised
			if (currSegmentIsNowStatic || (fingersRaised && listener->sampleSize() > 10))
			{
				cout << "Valid Segment with size: " << listener->sampleSize() << endl;

				//Segment should be classified
				listener->endSample();
				outStream->Clear();
				const char* gestrAction = "classify";
				vector<string> actionResult = listener->gestureAction(gestrAction, vector<string>());
				if (actionResult.size() > 2) //If the recognition is not "None"
				{
					sendGestrActionResults(gestrAction, actionResult);
				}
				listener->startSample(""); //Allow a new segment to begin.
			}
		}
	}
	void ProcessMessage(const osc::ReceivedMessage & m,
			const IpEndpointName & remoteEndpoint)
	{
		try
		{
			osc::ReceivedMessageArgumentIterator arg = m.ArgumentsBegin();
			if (strcmp(m.AddressPattern(), "/gestr/sample") == 0)
			{
				handleGestrSample(arg, m);
			}
			else if (strcmp(m.AddressPattern(), "/gestr/action") == 0)
			{
				handleGestrAction(arg, m);
			}
			else if (strcmp(m.AddressPattern(), "/tuio/2Dcur source ccv@localhost") == 0)
			{
				handleTUIO(arg, m);
			}
			else
			{
				cout << "Unknown Address Pattern: " << m.AddressPattern() << endl;
			}
		} catch (osc::Exception& e)
		{
			cout << "error while parsing message: " << m.AddressPattern()
					<< ": " << e.what() << endl;
		}
	}

	typedef map<string, vector<double> > paramValMapT;

	void sendStream()
	{
		UdpTransmitSocket socket = UdpTransmitSocket(IpEndpointName(ADDRESS,
				OUTPUT_PORT));
		socket.Send(outStream->Data(), outStream->Size());
		outStream->Clear();
	}

	bool isFingerLowered(vector<int> & currIds)
	{
		bool fingerLowered = false;
		for (size_t i = 0; i < currIds.size(); i++)
			if (find(liveIds.begin(), liveIds.end(), currIds[i]) == liveIds.end())
				fingerLowered = true;
		return fingerLowered;
	}
	bool isFingerRaised(vector<int> & currIds)
	{
		bool fingerRaised = false;
		if(currIds.size() == 0 && liveIds.size() > 0)
			fingerRaised = true;
		for (size_t i = 0; i < liveIds.size(); i++)
			//If this id from liveIds is NOT found in the current frame
			if (find(currIds.begin(), currIds.end(), liveIds[i]) == currIds.end())
				fingerRaised = true;
		return fingerRaised;
	}

	void cloneFrameToOutStream(const ContactSetFrame & frame)
	{
		for(size_t i = 0; i < frame.size(); i++)
			cloneContactToOutStream(frame.frame[i]);
	}

	void cloneContactToOutStream(const Contact & contact)
	{
		/*if (!outStream->IsBundleInProgress())
		{
			*outStream << osc::BeginBundleImmediate;
			cout << "Bundle was not in progress for contact" << endl;
		}*/
		*outStream << osc::BeginMessage("/tuio/2Dcur") << "set";
		*outStream << contact.id << contact.x << contact.y << contact.dx
				<< contact.dy << 0.0 << contact.width << contact.height;
		*outStream << osc::EndMessage;
	}

	Contact createContactFromOscArgs(
			osc::ReceivedMessageArgumentIterator & arg,
			const osc::ReceivedMessage & m)
	{
		long int numArgs = m.ArgumentCount();
		Contact contact;
		if(numArgs == 9) //Most probably from CCV (including the 'set' message)
		{
			contact.id = (arg++)->AsInt32();
			contact.x = (arg++)->AsFloat() * xRes;
			contact.y = (arg++)->AsFloat() * yRes;
			contact.dx = (arg++)->AsFloat() * xRes;
			contact.dy = (arg++)->AsFloat()  * yRes;
			arg++;
			contact.width = (arg++)->AsFloat()  * xRes;
			contact.height = (arg++)->AsFloat()  * yRes;
			contact.pressure = 0;
		}
		else if(numArgs == 7) //From TUIO Simulator (Profile: /tuio/2Dcur set s x y X Y m)
		{
			contact.id = (arg++)->AsInt32();
			contact.x = (arg++)->AsFloat() * xRes;
			contact.y = (arg++)->AsFloat() * yRes;
			contact.dx = (arg++)->AsFloat() * xRes;
			contact.dy = (arg++)->AsFloat()  * yRes;
			arg++; //We don't really use motion acceleration ... yet.
		}

		if (arg != m.ArgumentsEnd())
			throw osc::ExcessArgumentException();
		return contact;
	}
	vector<int> getCurrIds(osc::ReceivedMessageArgumentIterator arg,
			const osc::ReceivedMessage & m)
	{

		vector<int> currIds;
		while (arg != m.ArgumentsEnd())
		{
			int id = (arg++)->AsInt32();
			currIds.push_back(id);
		}
		return currIds;
	}

	virtual int sendOutStream(const ContactSetFrame & frame, vector<int> &ids, int &fseq)
	{
		if (!outStream->IsBundleInProgress()
					&& !outStream->IsMessageInProgress())
			*outStream << osc::BeginBundleImmediate;

		cloneFrameToOutStream(frame);

		*outStream << osc::BeginMessage("/tuio/2Dcur") << "alive";
		BOOST_FOREACH(int id, ids)
			*outStream << id;
		*outStream << osc::EndMessage;


		*outStream << osc::BeginMessage("/tuio/2Dcur") << "fseq" << fseq << osc::EndMessage;
		*outStream << osc::EndBundle;

		sendStream();

		return 1;
	}

	/**
	 * Generic method for any Gestr Action
	 */
	virtual int sendGestrActionResults(const char *actionString, const vector<string> actionResult)
	{
		outStream->Clear();
		if (!outStream->IsBundleInProgress())
			*outStream << osc::BeginBundleImmediate;

		*outStream << osc::BeginMessage("/gestr/action");
		cout << "Action performed: " << actionString << "\n\tResult: ";
		for (size_t i = 0; i < actionResult.size(); i++)
		{
			*outStream << actionResult[i].c_str();
			cout << actionResult[i] << ", ";
		}
		*outStream << osc::EndMessage;
		cout << endl;
		sendStream();

		return 1;
	}

	void sendTestMessage(const char *testString)
	{
		outStream->Clear();
		*outStream << osc::BeginBundleImmediate;
		*outStream << osc::BeginMessage("/gestr/action");
		*outStream << testString << osc::EndMessage;
		sendStream();
	}
	virtual int sendGestrParams(const paramValMapT namedParams)
	{
		if(namedParams.size() == 0)
			return 0;

//		outStream->Clear();
		if (!outStream->IsBundleInProgress())
			*outStream << osc::BeginBundleImmediate;

		/*bool exclusiveParam = false;
		BOOST_FOREACH(paramValMapT::value_type &namedParamValPair, namedParams)
		{
			if(namedParamValPair.first == "allMean")
			{
				cout << "Discarding other params. Using only allMean" << endl;
				exclusiveParam = true;
			}
		}

		if(exclusiveParam)
		{
			BOOST_FOREACH(paramValMapT::value_type &namedParamValPair, namedParams)
				if(namedParamValPair.first != "allMean")
					namedParamValPair.second.clear();
		}*/

		BOOST_FOREACH(const paramValMapT::value_type &namedParamValPair, namedParams)
		{
			bool allZero = true;

			//We want to send values each frame, whether or not the value is greater than zero.
//			BOOST_FOREACH(float paramVal, namedParamValPair.second)
//			{
//				allZero = paramVal == 0 ? true : false;
//			}
//			if(allZero)
//				continue; //Skip this param, has all zero values

			if(namedParamValPair.second.empty()) // No Vals
				continue;
			const string &name =  namedParamValPair.first;
			*outStream << osc::BeginMessage("/gestr/action");
			*outStream << "param_update";
			*outStream << name.c_str();
//			cout << name << "\t";
			BOOST_FOREACH(float paramVal, namedParamValPair.second) //using floats for javaosc compatibility
			{
//				cout << paramVal << "\t";
				*outStream << paramVal;
			}
//			cout << endl;
			*outStream << osc::EndMessage;
		}
		//sendStream();
		return 1;
	}

	/**
	 * Hook for overriding classes to use finger lowered event
	 */
	virtual int fingerLowered(){ return 0; }

	/**
	 * Hook for overriding classes to use finger raised event
	 */
	virtual int fingerRaised(){ return 0; }
};

void initMultitouchOscReceiver(OscHandler *oscHandler, OscGestureRecognition *collector)
{

	UdpListeningReceiveSocket s(IpEndpointName(IpEndpointName::ANY_ADDRESS,
				IN_PORT), oscHandler);
//	UdpListeningReceiveSocket s(IpEndpointName(IpEndpointName::ANY_ADDRESS,
//			IN_PORT), &oscListener);
//
	cout << "Now Listening for input on port " << IN_PORT << "..." << endl;

	cout << "Output packets to port: " << OUTPUT_PORT << endl;

	oscHandler->listener = collector;
	oscHandler->recognizer = &collector->recognizer;
	oscHandler->inSock = &s;
	oscHandler->sendTestMessage("This is a Test !!");
	oscHandler->run();
	cout << "Done listening to OSC.\n";

}

#endif /* MULTITOUCHOSCRECEIVER_H_ */
